#include <lidar.h>

int main(int argc, char** argv) {
    // 初始化 ROS 节点
    ros::init(argc, argv, "lidar_distance");
    ros::NodeHandle nh;

    std::string serial_port;
    int baud_rate;
    std::string pub_topic;

    nh.param<std::string>("serial_port", serial_port, "/dev/ttyUSB0");
    nh.param<int>("baud_rate", baud_rate, 115200);
    nh.param<std::string>("pub_topic", pub_topic, "lidar_distance");

    // 创建发布器
    ros::Publisher lidar_pub = nh.advertise<cv_joint_angle::Forearm>(pub_topic, 10);

    int fd;
    struct termios tty;
    char buffer[100];
    ssize_t bytesRead;

    // 发送的数据（可以自定义）
    unsigned char buf[] = {0x01, 0x03, 0x00, 0x00, 0x00, 0x01, 0x84, 0x0A};

    // 打开 /dev/ttyUSB0 串口端口
    while (true) {
        fd = open(serial_port.c_str(), O_RDWR | O_NOCTTY); // 修改为O_RDWR，以便读写操作

        if (fd == -1) {
            ROS_WARN_THROTTLE(10, "Unable to open serial port %s. Retrying...", serial_port.c_str());
            std::this_thread::sleep_for(std::chrono::seconds(1));
            continue;
        } else {
            ROS_INFO_ONCE("Open serial port %s.", serial_port.c_str());
            break;
        }
    }

    // 配置串口参数
    if (tcgetattr(fd, &tty) != 0) {
        ROS_ERROR("Error getting serial port attributes.");
        close(fd);
        return 1;
    }

    // 配置波特率
    cfsetospeed(&tty, B115200);
    cfsetispeed(&tty, B115200);

    tty.c_cflag |= (CLOCAL | CREAD);
    tty.c_cflag &= ~PARENB;
    tty.c_cflag &= ~CSTOPB;
    tty.c_cflag &= ~CSIZE;
    tty.c_cflag |= CS8;

    tty.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG);
    tty.c_iflag &= ~(INPCK | ISTRIP);
    tty.c_oflag &= ~OPOST;

    tty.c_cc[VMIN] = 1;
    tty.c_cc[VTIME] = 0;

    if (tcsetattr(fd, TCSANOW, &tty) != 0) {
        ROS_ERROR("Error setting serial port attributes.");
        close(fd);
        return 1;
    }

    // 循环发送和接收数据
    static double last_distance = -1;
    while (ros::ok()) {
        auto start_time = std::chrono::steady_clock::now();

        // 发送数据
        ssize_t bytesWritten = write(fd, buf, sizeof(buf));
        if (bytesWritten == -1) {
            ROS_ERROR("Error writing to serial port.");
            close(fd);
            return 1;
        }

        // 读取串口数据
        bytesRead = read(fd, buffer, sizeof(buffer));

        if (bytesRead == -1) {
            ROS_ERROR("Error reading serial port.");
            close(fd);
            return 1;
        }

        if (bytesRead > 0) {
            unsigned char highByte = buffer[3];
            unsigned char lowByte = buffer[4];
            double distance = ((highByte << 8) | lowByte) + forearm_distance_diff;

            // 输出实际的距离值
            // ROS_INFO("distance: %f", distance);

            // 创建并填充 Bucket 消息
            cv_joint_angle::Forearm msg;
            msg.header.stamp = ros::Time::now();
            if (last_distance != -1){
                if(std::abs(last_distance - distance) < 4.){
                    msg.cylinder_forearm = last_distance + 0.001 * (distance - last_distance);
                    last_distance = last_distance + 0.001 * (distance - last_distance);
                } else {
                    msg.cylinder_forearm = distance;
                    last_distance = distance;
                }
            }else{
                msg.cylinder_forearm = distance;
                last_distance = distance;
            }

            // 发布消息
            lidar_pub.publish(msg);
            ROS_INFO_THROTTLE(120, "Lidar distance node is running...");
        }
        else {
            ROS_WARN_THROTTLE(10, "No valid data received from the serial port.");
        }

        // 计算本次循环所用时间，并保持 50Hz 的发送频率（即每 20ms 发送一次数据）
        auto end_time = std::chrono::steady_clock::now();
        std::chrono::duration<double> elapsed_seconds = end_time - start_time;
        std::chrono::milliseconds sleep_duration(20);

        if (elapsed_seconds.count() < 0.02) {
            // 如果循环时间小于 20ms，等待剩余的时间
            std::this_thread::sleep_for(sleep_duration - elapsed_seconds);
        }

        // 回调函数执行ROS节点
        ros::spinOnce();
    }

    // 关闭串口
    close(fd);

    return 0;
}
